/*
 * @Date         2022-03-23 22:51:13
 * @LastEditors  Tennikl
 * @LastEditTime 2022-07-18 13:43:22
 * @FilePath     \D_SJK_2022\D_SJK_2022\USER\CODE\Balance\Balance.c
 */
/*
 * @File:
 * @Author: sjk
 * @Date: 2022-02-07 22:12:17
 * @LastEditTime 2022-05-13 15:35:24
 * @Brief:          控制：直立、速度、方向环
 * @Note:
 * 《智能车制作》
 */
#include "LQ_GPIO_LED.h"
#include "Balance.h"
#include "angle.h"
#include "SEEKFREE_MPU6050.h"
#include "LQ_PID.h"
#include "Motor.h"
#include "Encoder.h"
#include "Global_Var.h"
#include "Global_define.h"

//#define Angle_Set -target_angle // 根据陀螺仪安装角度矫正
// #define Gyroy_Set -8            // 陀螺仪 零点漂移
// #define Gyroz_Set -54          // 陀螺仪 零点漂移

#define Gyroy_Set Gyro_setoff[1]            // 陀螺仪 零点漂移
#define Gyroz_Set Gyro_setoff[3]          // 陀螺仪 零点漂移

#define PI 3.14159265
#define angleMax 8107
#define angleMin 150

#define SpeedLoopLimit 40

//GYRO模块
#define gyro_type 1
// 0:icm20602;
// 1:mpu6050_GY521;
// 2:mpu6050_JY61;
#if gyro_type == 0
    #define _icm20602
#elif gyro_type == 1
    #define _mpu6050_GY521
#elif gyro_type == 2
    #define _mpu6050_JY61
#endif

int Gyro_setoff[3] = {0};//

static u8 count_5ms  = 0;
static u8 count_10ms = 0;
static u8 count_20ms = 0;
static u8 count_LED  = 0;
static u8 count_Stop  = 0;

int16_t Enc_L,Enc_R;    //
int target_speed = 0;
int pwm_dir = 0;
int pwm_left = 0;
int pwm_right = 0;

int Motor_test_pwm = Motor_test_basic;
int Motor_test_dir = 20;

float gyro_y_fliter = 0;
/*------------------------- 直立环 变量 -------------------------*/
int target_angle = 660;    // 平衡角度  @键盘设置
int p_up = 140, d_up = 0; // 140    @键盘设置
float k_up = 1.7;         // 1.7    @键盘设置

int p_up_pre = 0, pre_angle = 0;

int error_angle = 0, error0_angle = 0, error1_angle = 0;
int pwm_up = 0;
int l = 0, r = 0;
/*------------------------- 速度环 变量 -------------------------*/
// int speed_OPEN = 0;   //速度环标志位
//int target_speed = 0; //目标速度
// int error_speed = 0, pv_speed = 0, pv_speed_l = 0, pv_speed_r = 0;
// int p_speed = 10, i_speed = 10;
int speed_now = 0;

// int stop_flag = 0; //(1是停车，0是启动)

/*------------------------- 普通直立环 -------------------------*/
void upright()
{
    error_angle = target_angle - angle_use + speed_now; // 40-50=-10
    pwm_up = (int)(p_up * error_angle + d_up * (error_angle - error0_angle) + k_up * mpu_gyro_y);

    //储存历史偏差
    error1_angle = error0_angle;
    error0_angle = error_angle;
}
//正常是0，电池着地是90
void upright_pre()
{
    error_angle = pre_angle - angle_use;
    pwm_up = (int)(p_up_pre * error_angle + k_up * mpu_gyro_y);
}

/*------------------------- 串级直立环  移植自龙邱-------------------------*/
pid_param_t Gyro_loop;  //角速度环 ——直立内环 ——位置式      5ms
pid_param_t Angle_loop;  //角度环   ——直立外环 ——位置式     10ms
pid_param_t Speed_loop;  //速度环             ——增量式      20ms
float Gyro_loop_out = 0;
float Angle_loop_out = 0;
float Speed_loop_out = 0;


/* //预留命名
pid_param_t Direction_exloop; //方向外环           ——位置式
pid_param_t Direction_inloop; //方向外环           ——位置式
*/

/**
 * @Brief:
 * @param {*}
 * @return {*}
 * @Note:
 *      直立的一些PID初始化
 *      可以先不要速度环
 *      先角度，再角速度
 *      在龙邱经验30min左右
 */
void Balance_init()
{
    PidInit(&Gyro_loop);
    PidInit(&Angle_loop);
    PidInit(&Speed_loop);
    //一定要逐步增大
    //角速度环  负  1000？   增幅0.1
    #if defined Gyro_Loop_PDLocCtrl
        Gyro_loop.kp  = 2.5;
        Gyro_loop.ki  = 0.04;
        Gyro_loop.kd  = 6;
        Gyro_loop.imax = 7000;
        // Gyro_loop.kp  = -2.8;
        // Gyro_loop.ki  = 0;
        // Gyro_loop.kd  = -2;
        // Gyro_loop.kp  = -2.8;
        // Gyro_loop.ki  = 0;
        // Gyro_loop.kd  = -1.32;
    
        //角度环    正  10°？
        Angle_loop.kp = 5;
        Angle_loop.ki = 0.001;
        Angle_loop.kd = 3;
        Angle_loop.imax = 2000;
        // Angle_loop.kp = 50;
        // Angle_loop.ki = 0.002;
        // Angle_loop.kd = 20;
        // Angle_loop.imax = 2000;
        //速度环    负   增幅0.01
        Speed_loop.kp = 0.3 ;
        Speed_loop.ki = 0;//002;
        Speed_loop.kd = 0.15;
        Speed_loop.imax = 100;
    #elif defined Gyro_Loop_PIIncCtrl
        Gyro_loop.kp  = -1.4;
        Gyro_loop.ki  = -0.12;
        Gyro_loop.kd  = -6;
        //Gyro_loop.imax = 300;

        //角度环    正  10°？
        Angle_loop.kp = 10;
        Angle_loop.ki = 0;
        Angle_loop.kd = 5;
        Angle_loop.imax = 2000;
        //速度环    负   增幅0.01
        Speed_loop.kp = -5;
        Speed_loop.ki = 0;
        Speed_loop.kd = -20;
    #endif

    //开启定时器
    //TIMER_InitConfig(TIMER_6, 5);
}

//平滑滤波
/**
 *  @brief      moving_filter//平滑滤波
 *  @param[in]  value   pull in new value.||滤波变量
 *  @param[in]  NumOf_Arragy    select numbers of filter arragy to save data, range from 0 to 7. ||滤波数组编号 0~7
 *  @param[in]  sampling_len    set the sampling length, range from 6 to 16. ||滤波采样长度 建议6~16
 *  @return     filter result.
 */
float moving_filter(float value, short NumOf_Arragy, short sampling_len)
{
    static float Filter_Arragy[8][17] = {0};
    short max = 16, min = 6;
    if (sampling_len > max)
        sampling_len = max;
    else if (sampling_len < min)
        sampling_len = min;

    float sum = 0;
    for (short i = sampling_len - 1; i > 0; i--)
    {
        Filter_Arragy[NumOf_Arragy][i] = Filter_Arragy[NumOf_Arragy][i - 1];
        sum += Filter_Arragy[NumOf_Arragy][i];
    }
    Filter_Arragy[NumOf_Arragy][0] = value; //将新的数据放置到 数据的最前面
    sum += value;
    Filter_Arragy[NumOf_Arragy][16] = sum / sampling_len;
    return Filter_Arragy[NumOf_Arragy][16];
}

/**
 * @Date2022-07-18 13:43:46
 * @decription     gyro零偏采样
 * @author         Tennikl
 * @versions       Ver 0.1
 * @return {*}
 */
void gyrosetoff()
{
    static int sum[3] = {0}, count = 0;
    static int Start_time = 1;
    static int Stop_time = 2;

    if (count_1s < Stop_time && count_1s >= Start_time)
    {
        sum[0] += mpu_gyro_x;
        sum[1] += mpu_gyro_y;
        sum[2] += mpu_gyro_z;
        ++count;
    }
    else if(count_1s == Stop_time && Gyro_setoff[0] == 0)
    {
        Gyro_setoff[0] = (int)(sum[0] > 0 ? sum[0] * 1.0 / count + 0.5 : sum[0] * 1.0 / count - 0.5);
        Gyro_setoff[1] = (int)(sum[1] > 0 ? sum[1] * 1.0 / count + 0.5 : sum[1] * 1.0 / count - 0.5);
        Gyro_setoff[2] = (int)(sum[2] > 0 ? sum[2] * 1.0 / count + 0.5 : sum[2] * 1.0 / count - 0.5);
    }
    FMQ(3);
}



/**
 * @Brief:
 * @param {*}
 * @return {*}
 * @Note:
 *     我的平衡中断，直接丢5ms中断里
 */
void Balance_IRQ() // 5ms
{
    // if(count_Stop>Stop_Time && Stop_Time != 0)
    // {
    //     Motor(0,0);
    //     LED_Ctrl(LEDALL,ON);
    //     return;
    // }
    
    //=====计时=============================
    count_5ms++;
    count_10ms++;
    count_20ms++;
    count_LED++;
    if (count_5ms == 1)
        count_5ms = 0;
    if (count_10ms == 2)
        count_10ms = 0;
    if (count_20ms == 4)
        count_20ms = 0;
    if (count_LED == 200)//1s
    {
        LED_Ctrl(LED0, RVS); // LED翻转闪烁 直观检查中断时间是否溢出
        count_LED = 0;
        count_Stop++;
        //图像帧率显示
        ++count_1s;
    }

    //=====编码器读取=============================

    //testdata[3] = PIN_Read(PB5);
    //testdata[4] = Read_Encoder(2);
    //testdata[5] = PIN_Read(PB7);
    //testdata[6] = Read_Encoder(4);
    //testdata[7] = Read_Encoder(5);
    Enc_L = Enc_left();
    Enc_R = Enc_right();
    speed_now = (Enc_L + Enc_R) >> 1;

    //=====角度角速度读取=============================
    //Test_LQDMP();
    LQ_DMP_Read(); 
    angle_use = (Roll*100 -angleMin) * 900 / (angleMax - angleMin);
    // if (angle_use > 100)
    //     angle_use = 100;
    if (angle_use < 0)
        angle_use = 0;
    //testdata[0]=(int)(Pitch*beishu);
    //testdata[1]=(int)(Roll*beishu);
    get_accdata(); //获取加速度数据   //mpu_gyro_x
    get_gyro();    //获取陀螺仪数据   //mpu_acc_x

    if (count_1s <= 3)
    {
        gyrosetoff();
        return;
    }
    //angle_senor();

    //三环计算
    if (Global_mode == Motor_test_mode)
    {
        Gyro_loop_out = 0;
    }
    else
    {
        if (count_5ms == 0)
        {
            gyro_y_fliter = (int)(moving_filter((float)(mpu_gyro_y - Gyroy_Set), 1, 6));//滤波角速度
            #if defined Gyro_Loop_PDLocCtrl
                Gyro_loop_out = PidLocCtrl(&Gyro_loop, - Angle_loop_out + (float)(gyro_y_fliter)); //角速度环输出
            #elif defined Gyro_Loop_PIIncCtrl
                Gyro_loop_out = PidIncCtrl(&Gyro_loop, Angle_loop_out - (float)(gyro_y_fliter)); //角速度环输出
                Gyro_loop_out = constrain_float(Gyro_loop_out, -5000, 5000);//限幅
                Gyro_loop.out = Gyro_loop_out;
            #endif
        }
        if (count_10ms == 0)
        {
            switch (Global_mode)
            {
            case Gyro_loop_test:
                Angle_loop_out = 0;
                break;
            case Angle_loop_test:case Speed_loop_test:case Image_test_mode:
            case Normal_mode:  
                Angle_loop_out = PidLocCtrl(&Angle_loop, (angle_use) - target_angle + Speed_loop_out); //角度环输出
                break;
            }
        }
        if (count_20ms == 0)
        {
            switch (Global_mode)
            {
            case Gyro_loop_test:case Angle_loop_test:
                Speed_loop_out = 0;
                break;
            case Speed_loop_test:case Image_test_mode:
            case Normal_mode:  
                Speed_loop_out = PidIncCtrl(&Speed_loop, target_speed - speed_now); //速度环输出
                Speed_loop_out = constrain_float(Speed_loop_out, -SpeedLoopLimit, SpeedLoopLimit);                      //速度环限幅
                Speed_loop.out = constrain_float(Speed_loop.out, -SpeedLoopLimit, SpeedLoopLimit);                      //速度环限幅
                break;
            }
        }
    }

    //电机输出
    if (Global_mode == Motor_test_mode)
    {
        pwm_up = Motor_test_pwm;
        pwm_dir = Motor_test_dir;

        pwm_left = (int)((100+pwm_dir)/100.0*pwm_up);
        pwm_left = constrain_float(pwm_left, -9800, 9800);
        pwm_right = (int)((100-pwm_dir)/100.0*pwm_up);
        pwm_right = constrain_float(pwm_right, -9800, 9800);
        Motor_on(pwm_left, pwm_right);
    }
    else
    {
        //pwm_up = constrain_float((int)Gyro_loop_out, (int)(-9800.0/abs(pwm_dir)*100), (int)(9800/abs(pwm_dir)*100));//限幅
        if (Stop_flag)
        {
            //Motor_on(0, 0); //停车
            Motor(0, 0);
        }
        else
        {
            //pwm_dir=0;//测试差异


            pwm_up = (int)Gyro_loop_out;
            pwm_left = (int)((100+pwm_dir)/100.0*pwm_up);
            pwm_right = (int)((100-pwm_dir)/100.0*pwm_up);

            pwm_right = constrain_float(pwm_right, -9800, 9800);
            pwm_left = constrain_float(pwm_left, -9800, 9800);
            Motor(pwm_left, pwm_right);
        }
    }



    //计时用
    count_5ms++;
    count_10ms++;
    count_20ms++;
    count_LED++;
    if (count_5ms == 1)
        count_5ms = 0;
    if (count_10ms == 2)
        count_10ms = 0;
    if (count_20ms == 4)
        count_20ms = 0;
    if (count_LED == 200)//1s
    {
        LED_Ctrl(LED0, RVS); // LED翻转闪烁 直观检查中断时间是否溢出
        count_LED = 0;
        count_Stop++;
        //图像帧率显示
        ++count_1s;
    }
}